import sys
from time import sleep
from leapc_cffi import libleapc, ffi
import math

# 手势检测

class LeapTracker:
    def __init__(self):
        self.connection = ffi.new("LEAP_CONNECTION *")
        self.last_position = None
        
    def initialize(self):
        # 创建并打开连接
        result = libleapc.LeapCreateConnection(None, self.connection)
        if result != 0:
            print("创建连接失败")
            return False
            
        result = libleapc.LeapOpenConnection(self.connection[0])
        if result != 0:
            print("打开连接失败")
            return False
            
        return True
        
    def get_tracking_frame(self):
        # 获取最新的追踪帧
        tracking_event = ffi.new("LEAP_TRACKING_EVENT *")
        result = libleapc.LeapPollConnection(self.connection[0], 
                                           ffi.sizeof("LEAP_CONNECTION_MESSAGE"),
                                           ffi.new("LEAP_CONNECTION_MESSAGE *"))
                                           
        if result != 0:
            return None
            
        result = libleapc.LeapGetFrame(self.connection[0], tracking_event)
        if result != 0:
            return None
            
        return tracking_event
        
    def calculate_direction(self, current_pos):
        if self.last_position is None:
            self.last_position = current_pos
            return None
            
        # 计算移动方向
        dx = current_pos[0] - self.last_position[0]
        dy = current_pos[1] - self.last_position[1]
        dz = current_pos[2] - self.last_position[2]
        
        # 更新上一次位置
        self.last_position = current_pos
        
        # 计算主要移动方向
        abs_dx = abs(dx)
        abs_dy = abs(dy)
        abs_dz = abs(dz)
        
        if max(abs_dx, abs_dy, abs_dz) < 5:  # 移动距离太小，认为静止
            return "静止"
            
        if abs_dx > abs_dy and abs_dx > abs_dz:
            return "向右" if dx > 0 else "向左"
        elif abs_dy > abs_dx and abs_dy > abs_dz:
            return "向上" if dy > 0 else "向下"
        else:
            return "向前" if dz < 0 else "向后"
            
    def run(self):
        if not self.initialize():
            return
            
        print("开始追踪物体移动...")
        try:
            while True:
                frame = self.get_tracking_frame()
                if frame and frame.tracking_frame_id > 0:
                    # 获取第一个追踪到的物体的位置
                    if frame.nHands > 0:
                        hand = frame.pHands[0]
                        position = (hand.palm.position.x, 
                                  hand.palm.position.y,
                                  hand.palm.position.z)
                        
                        direction = self.calculate_direction(position)
                        if direction:
                            print(f"移动方向: {direction}")
                
                sleep(0.1)  # 控制刷新率
                
        except KeyboardInterrupt:
            print("\n停止追踪")
        finally:
            libleapc.LeapCloseConnection(self.connection[0])

if __name__ == "__main__":
    tracker = LeapTracker()
    tracker.run()